package clubhouse.antislam;

public class AntiSLAM {
	
	private int scenario;
	private boolean enabled;

	public AntiSLAM(int scenario) {
		this.scenario = scenario;
		this.enabled = false;
	}

	public double[] applyAntiSlam(int nodeId, double[] initCoords, double x_robot, double y_robot)  {
		double [] result = null; 
		
		if (this.scenario==1){
			if (!this.enabled){
				if (x_robot<0) result = initCoords;
				else this.enabled = true;
			} 
			if (this.enabled){
				switch (nodeId){
				case 1:		//Node 1, (0,20) then (10,15)
					result = new double[]{10,15};
					break;
				case 2:		//Node 2, (10,15)
					result = new double[]{10,15};
					break;
				case 3:		//Node 3, (15,5)
					result = new double[]{15,5};
					break;
				case 4:		//Node 4, (5,5)
					result = new double[]{5,5};
					break;
				case 5:		//Node 5, do nothing
					result = initCoords;
					break;
				default:
					result = initCoords;
					break;
				}
			}
		} else {		//Scenario 2
			//TODO
		}
		return result;
	}
}
